/*
* wt_gyroscope.c
*
*  Created on: 2021年11月1日
*      Author: ggruler
*/

 #include "wt_gyroscope.h"

WITData WIT;


// ---------------------------------------------------------
//  @brief          维特陀螺仪初始化函数
//  @param
//  @return         void
//  @since          v1.0
//  Sample usage:   WIT_inf_init(&WIT);
//-----------------------------------------------------------
void WIT_inf_init(WITData* WITRecData)
{
    WITRecData->WITangle_Pitch=0;
    WITRecData->WITangle_Roll=0;
    WITRecData->WITangle_Yaw=0;
    
    WITRecData->WITangle_acc_x = 0;                 //加速度X轴
    WITRecData->WITangle_acc_y=0;                  //加速度Y轴
    WITRecData->WITangle_acc_z=0;                  //加速度Z轴
    
    WITRecData->WITangle_gyro_x=0;                 //陀螺仪X轴
    WITRecData->WITangle_gyro_y=0;                 //陀螺仪Y轴
    WITRecData->WITangle_gyro_z=0;                 //陀螺仪Z轴
    WITRecData->i=0;
    WITRecData->WITreceiveComplete_Flag=0;
    
    uart_init(GORY_UART,115200,GORY_UART_TX,GORY_UART_RX);      //gyro uart initialization
	uart_rx_interrupt(GORY_UART,ENABLE);                        //gyro uart rx enable
    interrupt_set_priority(GORY_UART_PRIORITY, WIT_priority);              //gyro uart set priority 2
}
// ---------------------------------------------------------
//  @brief          维特陀螺仪中断接受函数
//  @param
//  @return         void
//  @since          v1.0
//  Sample usage:   getWITData(UART_3, &WIT);
//-----------------------------------------------------------

void getWITData(uart_index_enum uartn, WITData* WITRecData)
{   
    uart_query(uartn, &WITRecData->rx[WITRecData->i]);                // 接收串口数据  存在在rx数组里
    WITRecData->i++;
    if (WITRecData->rx[0] != 0x55)
    {                                 //帧头检测 检测第1位
        WITRecData->i = 0;
        return;
    }
    else if (WITRecData->i<10) {return;}//数据不满11个，则返回
    else 
        if (WITRecData->rx[1] == 0x53)//帧头检测 检测第2位
        {
            
            if (WITRecData->i == 10 )
            {

                WITRecData->WITangle_Roll=((((short)WITRecData->rx[3]<<8)|WITRecData->rx[2])/32768.f*180.f);
                WITRecData->WITangle_Pitch=((((short)WITRecData->rx[5]<<8)|WITRecData->rx[4])/32768.f*180.f);
                WITRecData->WITangle_Yaw=((((short)WITRecData->rx[7]<<8)|WITRecData->rx[6])/32768.f*180.f);
                WITRecData->i = 0;
            }
        }else
            if(WITRecData->rx[1] == 0x51)
            {
                if (WITRecData->i == 10)
                {
                
                    WITRecData->WITangle_acc_x=((((int8)WITRecData->rx[2]<<8)|WITRecData->rx[3])/32768.f*16.f*9.8f);
                    WITRecData->WITangle_acc_y=((((int8)WITRecData->rx[4]<<8)|WITRecData->rx[5])/32768.f*16.f*9.8f);
                    WITRecData->WITangle_acc_z=((((int8)WITRecData->rx[6]<<8)|WITRecData->rx[7])/32768.f*16.f*9.8f);
                    WITRecData->i = 0;
                }
            }else 
                if(WITRecData->rx[1] == 0x52)
                {
                    if (WITRecData->i == 10)
                    {
                        
                        WITRecData->WITangle_gyro_x=((((int8)WITRecData->rx[3]<<8)|WITRecData->rx[2])/32768.f*2000);
                        WITRecData->WITangle_gyro_y=((((int8)WITRecData->rx[5]<<8)|WITRecData->rx[4])/32768.f*2000);
                        WITRecData->WITangle_gyro_z=((((int8)WITRecData->rx[7]<<8)|WITRecData->rx[6])/32768.f*2000);
                        WITRecData->i = 0;   
                        //Filter_average();
                    }
                }
                else
                {
                    WITRecData->i = 0;
                    return;
                }
    
    
}